﻿using System;
using System.Collections.Generic;
using System.Text;
using ElteIk.RoboSoccer.Core;
using ElteIk.RoboSoccer.Core.Physics;

namespace ElteIk.RoboSoccer.DavidTactics
{
    enum BlockType
    {
        EMPTY,
        TEAM,
        ENEMY,
        BALL
    }

    class Node
    {
        public Vector2D center;
        public float distance;
        public float estimate;
        public BlockType blocked;
        public Node parent;
        public List<Node> siblings;
        public int xIndex;
        public int yIndex;

        public Node()
        {
            center = new Vector2D(0.0f, 0.0f);
            siblings = new List<Node>();
        }

        public void NewFrame()
        {
            blocked = BlockType.EMPTY;
            parent = null;
        }

        public void Clear()
        {
            distance = +Utils.INFINITY;
            estimate = +Utils.INFINITY;
            parent = null;
        }
    }

    class Route : List<Vector2D>
    {
    }

    class NodeQueue
    {
        private List<Node> nodes;

        public NodeQueue()
        {
            nodes = new List<Node>();
        }

        public bool IsEmpty()
        {
            return nodes.Count == 0;
        }

        public Node PopSmallest()
        {
            float minValue = +Utils.INFINITY;
            Node minNode = null;
            int minIndex = -1;
            for (int i = 0; i < nodes.Count; ++i)
            {
                Node current = nodes[i];
                float currentValue = current.distance + current.estimate;
                if (currentValue < minValue)
                {
                    minValue = currentValue;
                    minNode = current;
                    minIndex = i;
                }
            }
            
            if (minIndex != -1)
                nodes.RemoveAt(minIndex);

            return minNode;
        }

        public void Push(Node node)
        {
            nodes.Add(node);
        }
    }

    class AStar
    {
        private RoboSoccerTeam team;
        private RoboSoccerTeam enemy;
        private SoccerField field;
        private int gX;
        private int gY;
        private float dX;
        private float dY;
        private Node[,] nodes;

        public AStar(
            SoccerField field,
            RoboSoccerTeam team,
            RoboSoccerTeam enemy,
            int gX,
            int gY
            )
        {
            this.field = field;
            this.team = team;
            this.enemy = enemy;
            this.gX = gX;
            this.gY = gY;
            dX = field.Width / gX;
            dY = field.Height / gY;
            nodes = new Node[gX, gY];

            for (int y = 0; y < gY; ++y)
                for (int x = 0; x < gX; ++x)
                    nodes[x, y] = new Node();

            for (int y = 0; y < gY; ++y)
                for (int x = 0; x < gX; ++x)
                {
                    Node current = nodes[x, y];

                    // Setup positions.
                    current.center.X = (x + 0.5f) * field.Width / gX;
                    current.center.Y = (y + 0.5f) * field.Height / gY;
                    current.xIndex = x;
                    current.yIndex = y;

                    // Setup siblings.
                    if (x > 0)
                        current.siblings.Add(nodes[x - 1, y]);
                    if (y > 0)
                        current.siblings.Add(nodes[x, y - 1]);
                    if (x < gX - 1)
                        current.siblings.Add(nodes[x + 1, y]);
                    if (y < gY - 1)
                        current.siblings.Add(nodes[x, y + 1]);
                    if (x > 0 && y > 0)
                        current.siblings.Add(nodes[x - 1, y - 1]);
                    if (x > 0 && y < gY - 1)
                        current.siblings.Add(nodes[x - 1, y + 1]);
                    if (x < gX - 1 && y > 0)
                        current.siblings.Add(nodes[x + 1, y - 1]);
                    if (x < gX - 1 && y < gY - 1)
                        current.siblings.Add(nodes[x + 1, y + 1]);
                }
        }

        public Route Search(Vector2D from, Vector2D to, bool avoidEnemy)
        {
            ClearNodes();
            Node fromGrid = AlignToGrid(from);
            Node toGrid = AlignToGrid(to);
            fromGrid.distance = 0;
            fromGrid.estimate = Heuristics(fromGrid, toGrid);
            NodeQueue queue = new NodeQueue();
            queue.Push(fromGrid);
            while (!queue.IsEmpty())
            {
                Node best = queue.PopSmallest();
                if (best == toGrid)
                    break;

                foreach (Node sibling in best.siblings)
                    if (!IsBlocked(sibling, avoidEnemy))
                    {
                        float newDistance = best.distance + Distance(best, sibling);
                        if (newDistance < sibling.distance)
                        {
                            sibling.distance = newDistance;
                            if (sibling.estimate == +Utils.INFINITY)
                                sibling.estimate = Heuristics(sibling, toGrid);
                            sibling.parent = best;
                            queue.Push(sibling);
                        }
                    }
            }

            if (toGrid.parent == null)
                return null; // No path found.

            // Create route.
            Route route = new Route();
            Node current = toGrid;
            while (current != null)
            {
                route.Add(current.center);
                current = current.parent;
            }
            route.Reverse();
            route.Add(to);
            
            return route;
        }

        public void NewFrame()
        {/*
            for (int y = 0; y < gY; ++y)
                for(int x = 0; x < gX; ++x)
                    nodes[x, y].NewFrame();

            foreach (RobotPlayer robot in enemy.Robots)
                AddRobot(robot, BlockType.ENEMY);
            foreach (RobotPlayer robot in team.Robots)
                AddRobot(robot, BlockType.TEAM);*/
        }

        private void AddRobot(RobotPlayer robot, BlockType blockType)
        {
            float radius = (float)(robot.Size * Math.Sqrt(2));
            Node topLeft = AlignToGrid(robot.Position - new Vector2D(radius, radius));
            Node bottomRight = AlignToGrid(robot.Position + new Vector2D(radius, radius));
            for (int y = topLeft.yIndex; y <= bottomRight.yIndex; ++y)
                for (int x = topLeft.xIndex; x <= bottomRight.yIndex; ++x)
                    if  ((nodes[x, y].center - robot.Position).Length < radius)
                        nodes[x, y].blocked = blockType;
        }

        private bool IsBlocked(Node node, bool avoidEnemy)
        {
            return
                node.blocked == BlockType.TEAM ||
                (node.blocked == BlockType.ENEMY && avoidEnemy);
        }

        private void ClearNodes()
        {
            for (int y = 0; y < gY; ++y)
                for (int x = 0; x < gX; ++x)
                    nodes[x, y].Clear();
        }

        private Node AlignToGrid(Vector2D v)
        {
            int indexX = (int)Math.Floor(v.X / field.Width * gX);
            int indexY = (int)Math.Floor(v.Y / field.Height * gY);
            if (indexX < 0) indexX = 0;
            if (indexX >= gX) indexX = gX - 1;
            if (indexY < 0) indexY = 0;
            if (indexY >= gY) indexY = gY - 1;
            return nodes[indexX, indexY];
        }

        private float Distance(Node n1, Node n2)
        {
            return Utils.Distance(n1.center, n2.center);
        }

        private float Heuristics(Node n1, Node n2)
        {
            return Distance(n1, n2);
        }
    }
}